#include "WeightCalculator.h"
#include "../occupancy_map.h"
#include <ros/ros.h>

WeightCalculator::WeightCalculator(void)
{
	sizeGrid			= SIZE_GRID;
	fattoreScalaZDem	= SCALE_FACTOR_Z_DEM;
	roverSize		= DIM_ROVER;

	computeWeight();
}

void WeightCalculator::computeWeight()
{
	weightUnevenZ = WEIGHT_UNEVEN_Z;
	weightFlat     = WEIGHT_FLAT;
	weightFlatDiag = WEIGHT_FLAT_DIAG;
	weightRotation   = WEIGHT_ROTATION;

}

#define DIST_OBSTACLE 5 //(DIM_ROVER + 30)

long WeightCalculator::distBetween(PathPoint *pos1, PathPoint *pos2, 
		const OccupancyMap &map, bool goal)
{
	long totalCost = 0;
	long translationCost = 0;
	long rotationCost = 0;
	long occupancyCost = 0;

	long occ_prob = 0;
	int distobstacle = 0;


	if (map.getGridXY(pos2->getX(), pos2->getY()) > FREE) {
		occ_prob = map.getGridXY(pos2->getX(), pos2->getY());
		if (occ_prob == OCCUPIED) {
      return MIN_LONG;
		}
	}

	distobstacle = DIST_OBSTACLE;

	double weightCloseness=0;

   for (int i=-distobstacle;i<distobstacle;++i)
    {
    	for (int j= -distobstacle ; j<distobstacle; ++j)
    	{
			  int dem_val = map.getGridXY(pos2->getX()+i, pos2->getY()+j);
				if (dem_val >= UNKNOWN)
    		{
    			weightCloseness += dem_val;
    		}

    	}
    }
   for (int i=-2;i<2;++i)//8 for corridors is more or less ok
    {
    	for (int j= -2 ; j<2; ++j)
    	{
    		int dem_val = map.getGridXY(pos2->getX()+i, pos2->getY()+j);
    		if (dem_val == OCCUPIED)
    		{
    			totalCost = MIN_LONG;
    		}

    	}
    }
    if (totalCost != MIN_LONG)
    {
			double weightDisplacement = 1.5;
			double weightRotation = 2.0;
			double weightOccupancy = 50.0;
			translationCost = round(weightDisplacement * horizontalCost(pos1, pos2));
			rotationCost = round(weightRotation * getRotationCost(pos1, pos2));
			occupancyCost = round(weightOccupancy * occ_prob);

			totalCost = translationCost + rotationCost + occupancyCost + weightCloseness; 
		}

	return totalCost;
}


long WeightCalculator::horizontalCost(PathPoint *pos1,
		PathPoint *pos2)
{
	int orientPos2 = pos2->getAngle();

	if ((orientPos2==0) || (orientPos2==2) || (orientPos2==4) || (orientPos2==6))
		return weightFlat;
	else
		return weightFlatDiag;
}


long WeightCalculator::unevenZcost(PathPoint *pos1, PathPoint *pos2,
		const OccupancyMap &map)
{
	long zPos1 = map.getGridXY(pos1->getX(), pos1->getY());
	long zPos2 = map.getGridXY(pos2->getX(), pos2->getY());

	long zDiff = fabs(zPos2 - zPos1);

	return zDiff;
}


long WeightCalculator::getRotationCost(PathPoint *pos1, PathPoint *pos2)
{
	long temp;
	int rotaz;
	int orientPos1 = pos1->getAngle();
	int orientPos2 = pos2->getAngle();

	rotaz = fabs( orientPos1 - orientPos2 );
	if (rotaz > 4)
	{
		temp = rotaz - 4;
		rotaz = 4 - temp;
	}

	return rotaz;
}

WeightCalculator::~WeightCalculator(void)
{
}
